#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
int main(int argc, char **argv)
{
  ros::init(argc, argv, "moveit_custom_demo");
  ros::NodeHandle node_handle; 
  ros::AsyncSpinner spinner(1);
  spinner.start();

  static const std::string PLANNING_GROUP = "arm";
  moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);

  const robot_state::JointModelGroup* joint_model_group =
  group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  //initialize visual tools
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
  visual_tools.deleteAllMarkers();
  visual_tools.loadRemoteControl();

  // 设置机器人终端的目标位置
  geometry_msgs::Pose target_pose1;

  target_pose1.orientation.w = 0.726282;
  target_pose1.orientation.x= 0;
  target_pose1.orientation.y = -0.687396;
  target_pose1.orientation.z = 0;

  target_pose1.position.x = 0.0;
  target_pose1.position.y = 0.2;
  target_pose1.position.z = 0.5;

  group.setPoseTarget(target_pose1);

  // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  auto tic = ros::Time::now();
  moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);
  auto toc = ros::Time::now() - tic;

  ROS_INFO("Online Execute time %.2fms", toc.toSec() * 1000);
  ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");   

  ROS_INFO_NAMED("trajectory_show", " the plan is %s",success?"succeed":"failed");

  visual_tools.publishAxisLabeled(target_pose1, "pose1");
  // visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
  
  if(visual_tools.publishTrajectoryLine(my_plan.trajectory_,  joint_model_group))
    ROS_INFO("Vision ok");
  visual_tools.trigger();
  visual_tools.prompt("show trajectory");

  //让机械臂按照规划的轨迹开ss始运动。
  if(success)
      group.execute(my_plan);
  ros::shutdown(); 
  return 0;
}


// // 包含miveit的API头文件
// #include <moveit/move_group_interface/move_group_interface.h>

// int main(int argc, char **argv)
// {
//   ros::init(argc, argv, "moveit_custom_demo");
//   ros::NodeHandle node_handle; 
//   ros::AsyncSpinner spinner(1);
//   spinner.start();

//   moveit::planning_interface::MoveGroupInterface group("arm");

//   // 设置机器人终端的目标位置
//   geometry_msgs::Pose target_pose1;
//   target_pose1.orientation.w = 0.726282;
//   target_pose1.orientation.x= 0;
//   target_pose1.orientation.y = -0.687396;
//   target_pose1.orientation.z = 0;

//   target_pose1.position.x = 0.0;
//   target_pose1.position.y = 0.2;
//   target_pose1.position.z = 0.5;
//   group.setPoseTarget(target_pose1);

//   // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
//   moveit::planning_interface::MoveGroupInterface::Plan my_plan;
//   moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan);

//   ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");   

//   //让机械臂按照规划的轨迹开始运动。
//   if(success)
//       group.execute(my_plan);

//   ros::shutdown(); 
//   return 0;
// }
